# -*- coding: utf-8 -*-
"""
Created on Fri Sep  6 16:09:44 2024

@author: LENOVO
"""

import matplotlib.pyplot as plt
import numpy as np
from sympy import *
from scipy.optimize import root, fsolve
import pandas as pd

plt.rc('font',family='SimHei')
plt.rc('font',size=16)
plt.rc('axes',unicode_minus=False)

#常数的确定
a=16*55/100  #m
b=55/(2*np.pi)/100 #m
vh=1 #m/s
L0=(341-27.5*2)/100
Lb=(220-27.5*2)/100
c=442.4836510502367 #待定系数C
# theta=np.linspace(0,32*np.pi,30*180)
r=lambda theta:((a-b*(theta)))


# s=lambda theta:a*theta-b*theta**2/2

thetas=lambda t:(a-np.sqrt(a**2-2*b*vh*t))/b #猜测角度值

def theta(t):
    f=lambda x:(b*x-a)*np.sqrt((a-b*x)**2+b**2)/(2*b)-0.5*b*np.log(np.sqrt((a-b*x)**2+b**2)+a-b*x)+442.4836510502367-t
    theta=root(f,thetas(t))
    theta=theta.x[0]
    return theta

#循环遍历
# for i in range(300,-1,-1):    #300s到0s循环遍历
#第i秒位置函数

def position(i):
    Data=[]
    THETA=[]
    X=[]
    Y=[]
    # y=np.ones((301,1))
    
    THETA.append(theta(i))
    X.append(r(THETA[0])*np.cos((THETA[0])))
    Y.append(-r(THETA[0])*np.sin((THETA[0])))

    Data.append(theta(i))
    f=lambda thetai:(r(THETA[0]))**2+(r(thetai))**2-L0**2-2*(r(THETA[0]))*(r(thetai))*np.cos(THETA[0]-thetai)
    thetai=root(f,THETA[0]-0.5)
    thetai=thetai.x[0]
    
    # print("theta1:",thetai)
    THETA.append(thetai)
    X.append(r(THETA[1])*np.cos((THETA[1])))
    Y.append(-r(THETA[1])*np.sin((THETA[1])))
    Data.append(thetai)
    
    for j in range(0,222,1):
        
        f=lambda thetai:(r(THETA[1+j]))**2+(r(thetai))**2-Lb**2-2*(r(THETA[1+j]))*(r(thetai))*np.cos(THETA[1+j]-thetai)
        thetai=root(f,THETA[1+j]-0.5)
        thetai=thetai.x[0]
        THETA.append(thetai)
        X.append(r(THETA[j+2])*np.cos((THETA[j+2])))
        Y.append(-r(THETA[j+2])*np.sin((THETA[j+2])))

    A=np.column_stack((X, Y, THETA))

    return A
# WZ=position(400)

def Velocity(i):
    P=position(i)
    V=[]
    V.append(1)
    for j in range(1,224):
        L=np.array([P[j-1,0]-P[j,0],P[j-1,1]-P[j,1]])
        H=np.array([1/np.sqrt(1**2+(Slope(P[j-1,2]))**2),Slope(P[j-1,2])/np.sqrt(1**2+Slope(P[j-1,2])**2)])
        T=np.array([1/np.sqrt(1**2+(Slope(P[j,2]))**2),Slope(P[j,2])/np.sqrt(1**2+Slope(P[j,2])**2)])
        # V1=np.array([1,H])
        # V2=np.array([v,T])
        f=lambda v:(V[j-1]*H[0]*L[0]+V[j-1]*H[1]*L[1])-(v*T[0]*L[0]+v*T[1]*L[1])
        v=root(f,V[j-1]-0.01)
        v=v.x[0]
        V.append(abs(v))
    return V

def Slope(ta):
    k=(-a*np.cos(ta)+b*np.sin(ta)+b*ta*np.cos(ta))/(-a*np.sin(ta)-b*np.cos(ta)+b*ta*np.sin(ta))
    return k


def XL(t):
    for i in range(0,223,1):
        Storage=position(t)
        k.append(Slope(Storage[i,2]))
    return k

# k=XL(300)
# print(k)

##第t秒时候的图像
def graph(t):
    Storage=position(t)
    # THETA=Storage[:,2]
    # # if (theta<Storage(i)&theta>Storage(i-1):
    # print(THETA)
    x_coords=Storage[:,0]
    y_coords=Storage[:,1]
    plt.plot(x_coords, y_coords,lw=3,c="r", marker='o',label='板凳龙')
    # ax.set_theta_direction(-1)
    # plt.plot(theta,r(theta),lw=1,c='r', label='原始数据点')
    plt.xlabel('横坐标(m)')
    plt.ylabel('纵坐标(m)')
    plt.legend()
    plt.title('t=412.473838s时各节点位置')
    plt.grid()  
    plt.show()

# line(1)


#分段函数表达式
# def line (t):
#     i=0
#     Storage=position(t)
#     FAI=Storage[0,2]
#     # for fai in range(FAI,0,-0.1):
#     fai=np.linspace(FAI,0,int(FAI)*10)
#     if (fai<=Storage(i,2) and fai>Storage(i+1,2)):
#         y=lambda x:k[i]*(x-Storage[i,0])+Storage[i,1]
#     else:
#         y=lambda x:k[i-1]*(x-Storage[i-1,0])+Storage[i-1,1]
#         i=i-1
#     fai=np.linspace(FAI,0,int(FAI)*10)
#     x=r(fai)*np.cos(fai)      
#     plt.plot(x,y(x),lw=3,c="r", marker='o')
# line(300)
        

def Head(t):
   xh0=27.5/100
   yh0=15/100
   P=position(t)
   # xt0=-27.5/100
   # yt0=15/100
   c=P[0,0]-P[1,0]
   k0=(P[0,1]-P[1,1])/(P[0,0]-P[1,0])
   a1=1/(np.sqrt(1+k0**2)) #cos
   a2=k0/(np.sqrt(1+k0**2)) #sin
   a1=c*a1/abs(c)
   a2=c*a2/abs(c)
   # A=np.array([[a1,-a2],[a2,a1]])
   xh=xh0*a1-yh0*a2+r(theta(t))*np.cos(theta(t))
   yh=xh0*a2+yh0*a1-r(theta(t))*np.sin(theta(t))
   H=np.array([xh,yh])
   return H

def Tail(t):
   # xh0=(341-27.5)/100
   # yh0=15/100
   xt0=-((341-27.5)/100)
   yt0=15/100
   P=position(t)
   c=P[0,0]-P[1,0]
   k0=(P[0,1]-P[1,1])/(P[0,0]-P[1,0])
   a1=1/(np.sqrt(1+k0**2)) #cos
   a2=k0/(np.sqrt(1+k0**2)) #sin
   # A=np.array([[a1,-a2],[a2,a1]])
   a1=c*a1/abs(c)
   a2=c*a2/abs(c)
   xt=xt0*a1-yt0*a2+r(theta(t))*np.cos(theta(t))
   yt=xt0*a2+yt0*a1-r(theta(t))*np.sin(theta(t))
   T=np.array([xt,yt])
   return T

def distanceH(t):
    d=[]
    H=Head(t)
    P=position(t)
    for i in range(1,223):
       k=(P[i,1]-P[i+1,1])/(P[i,0]-P[i+1,0])
       distance=abs(k*(H[0]-P[i,0])-H[1]+P[i,1])/np.sqrt(k**2+1)
       d.append(distance)
    return min(d)

def distanceT(t):
    d=[]
    T=Tail(t)
    P=position(t)
    for i in range(2,223):
       k=(P[i,1]-P[i+1,1])/(P[i,0]-P[i+1,0])
       distance=abs(k*(T[0]-P[i,0])-T[1]+P[i,1])/np.sqrt(k**2+1)
       d.append(distance)
    return min(d)
# f=lambda t:distance(t)-0.15
# t=root(f,)
# t=t.x[0]
# print(t)
# for i in range(300,443,1):
#     s1=distanceH(i)-0.15
#     s2=distanceT(i)-0.15
#     if (s1<0 or s2<0):
#         print(i)
#         break
#变步长
for i in np.linspace(412,414,500):
    s1=distanceH(i)-0.15
    s2=distanceT(i)-0.15
    if (s1<0 or s2<0):
        print(i)
        break
      
# for i in range(300,443,1):
#     s=distanceT(i)-0.15
#     if (s<0):
#         print(i)
#         break    


#解非线性方程
s1=lambda i:distanceH(i)-0.15
t=root(s1,413)
t=t.x[0]
print(t)
# print(graph(t))

# result2=[]
# for i in range(0,224):
#     P=position(t)
#     V=Velocity(t)
#     result2.append(P[i,0])
#     result2.append(P[i,1])
#     result2.append(V[i])

# result2=np.array(result2)
# result2=result2.reshape(224,3)
# result2=pd.DataFrame(result2)
# result2.to_excel("T2附件.xlsx",index=False,header=False)

# s1=lambda i:distanceT(i)-0.15
# t=root(s1,304)
# t=t.x[0]
# print(t)

        


        


